/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/** 
 * \file
 * \brief HubCtrl Processing parts - implementation.
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 * \todo Message authentication code is being repeated; consider refactoring 
 *       to do common parts only once.
 */
//*****************************************************************************

#include "hubctrl_process.h"
#include "owndebug.h"
#include "ownutils.h"
#include "../GIMnetAP_version.h"
#include "apsecurity.h"
#include <assert.h>

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

EProtocolExit processRequestQueryLocalNodeId(THubCtrl *hc,
                                             const struct SProcessQueryData *pqd)
{
  const uint32_t nodeid = pqd->listentry->connectionID;
  const uint32_t linkd_us = pqd->listentry->link_delay_us;
  
  
  uint8_t *replybuffer = pqd->replybuffer;
  THubCtrlProtocolHeader *replyhdr = (THubCtrlProtocolHeader *)replybuffer;
  THubCtrlSubHeaderNodeInfo *nodeinfo = 
    (THubCtrlSubHeaderNodeInfo *)(replybuffer+sizeof(THubCtrlProtocolHeader));
  
  // Construct reply mainhdr.
  const THubCtrlProtocolHeader *reqhdr = pqd->reqhdr;
  replyhdr->protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  replyhdr->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) + 
                                       sizeof(THubCtrlSubHeaderNodeInfo) );
  replyhdr->request = reqhdr->request;
  replyhdr->subheader_count = 1;
  replyhdr->flags = KHubCtrlFlagReply;
  replyhdr->result = KHubCtrlResultOK;
  replyhdr->message_id = reqhdr->message_id;

  // Construct data.
  nodeinfo->subtype = KHubCtrlSubtypeNodeInfo;
  nodeinfo->subsize = sizeof(THubCtrlSubHeaderNodeInfo);
  nodeinfo->nodeid = nodeid;
  nodeinfo->link_delay_us = linkd_us;


  // Store size to caller.
  *pqd->replybuffer_datasize = replyhdr->protocol_id.total_size;

  // Tests
  dPrint(ODTEST,"Requested LocalNodeId; result nodeID: 0x%08x", nodeinfo->nodeid);
  dPrint(ODTEST,"Requested LocalNodeId; result link_delay_us: %u us", nodeinfo->link_delay_us);


  return protocolExitNoError;
}
//*****************************************************************************

EProtocolExit processRequestServerEcho(THubCtrl *hc,
                                       const struct SProcessQueryData *pqd)
{
  uint8_t *replybuffer = pqd->replybuffer;

  // Check that incoming data fits outgoing data.
  if (pqd->req_size < pqd->replybuffer_size &&
      pqd->reqhdr->subheader_count >= 1) {

    // Copy input as is.
    memcpy(replybuffer, pqd->reqhdr, pqd->req_size);
    
    // Create pointer.
    THubCtrlProtocolHeader *replyhdr = (THubCtrlProtocolHeader *)replybuffer;

    // Construct reply mainhdr.
    //const THubCtrlProtocolHeader *reqhdr = pqd->reqhdr;
    replyhdr->flags = KHubCtrlFlagReply;
    replyhdr->result = KHubCtrlResultOK;

    // Store size to caller.
    *pqd->replybuffer_datasize = replyhdr->protocol_id.total_size;
    
    // Tests
    dPrint(ODTEST, "Requested Server ECHO, replying with same frame + REPLY flag");
    
    return protocolExitNoError;
  }

  return protocolExitError;
}
//*****************************************************************************

EProtocolExit processRequestHubVersion(THubCtrl *hc,
                                       const struct SProcessQueryData *pqd)

{
  uint8_t *replybuffer = pqd->replybuffer;
  THubCtrlProtocolHeader *replyhdr = (THubCtrlProtocolHeader *)replybuffer;
  THubCtrlSubHeaderVersion *nodever = 
    (THubCtrlSubHeaderVersion *)(replybuffer+sizeof(THubCtrlProtocolHeader));
  
  // Construct reply mainhdr.
  const THubCtrlProtocolHeader *reqhdr = pqd->reqhdr;
  replyhdr->protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  replyhdr->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) + 
                                       sizeof(THubCtrlSubHeaderVersion) );
  replyhdr->request = reqhdr->request;
  replyhdr->subheader_count = 1;
  replyhdr->flags = KHubCtrlFlagReply;
  replyhdr->result = KHubCtrlResultOK;
  replyhdr->message_id = reqhdr->message_id;

  // Construct data.
  nodever->subtype = KHubCtrlSubtypeVersion;
  nodever->subsize = sizeof(THubCtrlSubHeaderVersion);
  nodever->major        = GIMNETAP_VERSION_MAJOR;
  nodever->minor        = GIMNETAP_VERSION_MINOR;
  nodever->sub          = GIMNETAP_VERSION_SUB;
  nodever->fix          = GIMNETAP_VERSION_FIX;
  nodever->build        = GIMNETAP_VERSION_BUILD;
  nodever->protocol_ver = GIMNETAP_PROTOCOLVERSION;

  // Store size to caller.
  *pqd->replybuffer_datasize = replyhdr->protocol_id.total_size;

  // Tests
  dPrint(ODTEST, "Requested HubVersion; result %d.%d.%d.%d (%d) (Proto %d)",
         nodever->major,
         nodever->minor,
         nodever->sub,
         nodever->fix,
         nodever->build,
         nodever->protocol_ver);



  return protocolExitNoError;
}
//*****************************************************************************

EProtocolExit processRequestRegisterAP(THubCtrl *hc,
                                       const struct SProcessQueryData *pqd)

{
  EProtocolExit result = protocolExitError;
  const THubCtrlProtocolHeader *reqhdr = pqd->reqhdr;
  const unsigned int reqsize = pqd->req_size;
  const THubCtrlSubHeaderRegisterAP *rap = NULL;
  const THubCtrlSubHeaderAuthentication *auth = NULL;

  // Construct pointers to different elements. These are all
  // calculated compile-time.
  uint8_t *replybuffer = pqd->replybuffer;
  THubCtrlProtocolHeader *rbase = (void*)replybuffer;
  THubCtrlSubHeaderRegisterAP *rreg = (void*)(replybuffer+sizeof(THubCtrlProtocolHeader));
  THubCtrlSubHeaderAuthentication *rauth = (void*)( replybuffer + 
                                                    sizeof(THubCtrlProtocolHeader) + 
                                                    sizeof(THubCtrlSubHeaderRegisterAP) );
  
  //////////
  // Construct reply mainhdr (Must be done; not depending on result)
  rbase->protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  rbase->flags = KHubCtrlFlagReply;
  rbase->request = reqhdr->request;
  rbase->message_id = reqhdr->message_id;

   
  //////////
  // We have subheaders; So identify correct ones.
  if (pqd->subhdrcount == 2) {
    int i;
    for(i=0; i < 2; ++i) {
      THubCtrlSubHeaderIdentificationHeader *idh = pqd->subhdrtable[i];
      switch(idh->subtype) {
      case KHubCtrlSubtypeRegisterAP:
        rap = (void*)idh;
        break;
      case KHubCtrlSubtypeAuthentication:
        auth = (void*)idh;
        break;
      }
    }
  }

  //////////
  // Now, check that we got both headers (Authentication is always required)
  if (rap && auth) {
    const int am = hubctrlcommonAuthenticateMessage((void*)reqhdr, reqsize, auth,
                                                    hc->ap2ap_secret, hc->ap2ap_secret_size,
                                                    hc->timestamp_difference_max);
    if (am == 0) {
      //      dPrint(ODTEST,"MESSAGE IS AUTHENTIC!");
      // Message is authentic! So; verification steps completed. Now,
      // carry on with the real assignment.
      
      // 1. Mark the entry as Authenticated AP2AP link!
      OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkHub2Hub);
      OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkAuthenticated);

      // 2. Register the HubID to routing table.
      const int rsr = routingSetRoute(rap->peer_id, pqd->listentry->connectionID);

      // 3. Check routing operation
      // Based on the result from routing module, set the reply value
      if (rsr == 0) {
        dPrintLCGreen(ODTEST,"Registering inbound AP2AP link to %08x succeeded",
               rap->peer_id);
        rbase->result = KHubCtrlResultOK;

      } else {
        dPrint(ODTEST,"Registering inbound AP2AP link to %08x failed!",
               rap->peer_id);
        rbase->result = KHubCtrlResultIdReserved;
      }
      
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) +
                                        sizeof(THubCtrlSubHeaderRegisterAP) +
                                        sizeof(THubCtrlSubHeaderAuthentication) );
      rbase->subheader_count = 2;
      
      // Copy the RegisterAP as-is.
      *rreg = *rap;

      // .. but override the peer with local
      rreg->peer_id = connectionListGetLocalHubID(pqd->connectionlist);
      
      // Update the authentication. (Server verifies
      rauth->subtype = KHubCtrlSubtypeAuthentication;
      rauth->subsize = sizeof(THubCtrlSubHeaderAuthentication);
      rauth->timestamp_s = time(NULL);
      memset(rauth->sha1, 0x00, sizeof(rauth->sha1));
      uint8_t sha1[20];
      unsigned int sha1_size = 20;
      apsecurityCalculateHMAC(sha1,
                              &sha1_size,
                              (uint8_t*)hc->ap2ap_secret,
                              hc->ap2ap_secret_size,
                              replybuffer,
                              rbase->protocol_id.total_size);
      assert(sha1_size == 20);
      memcpy(rauth->sha1, sha1, sizeof(sha1));
      
    } else if (am > 0) {
      // Positive value was less critical error; meaning expired timestamp
      dPrint(ODTEST,"Message had expired timestamp!");
      rbase->result = KHubCtrlResultAuthExpired;
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
      rbase->subheader_count = 0;
      
    } else {
      dPrint(ODTEST,"Message is corrupted or compromised!");
      // Negative value is hard error - something is fucked up.
      rbase->result = KHubCtrlResultAuthFailed;
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
      rbase->subheader_count = 0;
      
    }
    
  } else {
    rbase->result = KHubCtrlResultUnknownError;
    rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
    rbase->subheader_count = 0;
    
  }

  // Now, write the size out to enable sending reply.
  *pqd->replybuffer_datasize = rbase->protocol_id.total_size;
   
  return result;
}
//*****************************************************************************

EProtocolExit processRequestRegisterAPReply(THubCtrl *hc,
                                            const struct SProcessQueryData *pqd)
{
  EProtocolExit result = protocolExitError;
  const THubCtrlProtocolHeader *reqhdr = pqd->reqhdr;
  const unsigned int reqsize = pqd->req_size;
  const THubCtrlSubHeaderRegisterAP *rap = NULL;
  const THubCtrlSubHeaderAuthentication *auth = NULL;

  //////////
  // We have subheaders; So identify correct ones.
  if (pqd->subhdrcount == 2) {
    int i;
    for(i=0; i < 2; ++i) {
      THubCtrlSubHeaderIdentificationHeader *idh = pqd->subhdrtable[i];
      switch(idh->subtype) {
      case KHubCtrlSubtypeRegisterAP:
        rap = (void*)idh;
        break;
      case KHubCtrlSubtypeAuthentication:
        auth = (void*)idh;
        break;
      }
    }
  } else {
    dPrintLCRed(ODTEST,"Required 2 subheaders, but %d found! Error!",
                pqd->subhdrcount);

  }

  //////////
  // Now, check that we got both hgeaders (Authentication is always required)
  if (rap && auth) {
    const int am = hubctrlcommonAuthenticateMessage((void*)reqhdr, reqsize, auth,
                                                    hc->ap2ap_secret, hc->ap2ap_secret_size,
                                                    hc->timestamp_difference_max);
    if (am == 0) {
      //      dPrint(ODTEST,"MESSAGE IS AUTHENTIC!");
      // Message is authentic! So; verification steps completed. Now,
      // carry on with the real assignment.
      
      if (reqhdr->result == KHubCtrlResultOK) {
        // The APID of the remote is:
        

        // 1. Mark the entry as Authenticated AP2AP link!
        OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkHub2Hub);
        OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkAuthenticated);


        // 2. Register the HubID to routing table.
        const int rsr = routingSetRoute(rap->peer_id, pqd->listentry->connectionID);
        
        // 2. Register route
        // Based on the result from routing module, set the reply value
        if (rsr == 0) {
          dPrintLCGreen(ODTEST,"Registering outbound AP2AP link to %08x succeeded",
                 rap->peer_id);
          
        } else {
          dPrint(ODTEST,"Registering outbound AP2AP link to %08x failed!",
                 rap->peer_id);

        }
        
      } else {
        dPrint(ODTEST,"Failed to construct AP2AP link :(");

      }

    } else if (am > 0) {
      // Positive value was less critical error; meaning expired timestamp
      dPrint(ODTEST,"Message had expired timestamp!");
      
    } else {
      dPrint(ODTEST,"Message is corrupted or compromised!");
    }
    
  } else {
    
    dPrint(ODTEST,"Unknown error");
    
  }
  
  
  return result;
}
//*****************************************************************************

EProtocolExit processRequestRegisterAPService(THubCtrl *hc,
                                              const struct SProcessQueryData *pqd)
  
{
  EProtocolExit result = protocolExitError;
  const THubCtrlProtocolHeader *reqhdr = pqd->reqhdr;
  const unsigned int reqsize = pqd->req_size;
  const THubCtrlSubHeaderRegisterAPService *raps = NULL;
  const THubCtrlSubHeaderAuthentication *auth = NULL;

  // Construct pointers to different elements. These are all
  // calculated compile-time.
  uint8_t *replybuffer = pqd->replybuffer;
  THubCtrlProtocolHeader *rbase = (void*)replybuffer;
  THubCtrlSubHeaderRegisterAPService *rregs = (void*)(replybuffer+sizeof(THubCtrlProtocolHeader));
  THubCtrlSubHeaderAuthentication *rauth = (void*)( replybuffer + 
                                                    sizeof(THubCtrlProtocolHeader) + 
                                                    sizeof(THubCtrlSubHeaderRegisterAPService) );
  
  //////////
  // Construct reply mainhdr (Must be done; not depending on result)
  rbase->protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  rbase->flags = KHubCtrlFlagReply;
  rbase->request = reqhdr->request;
  rbase->message_id = reqhdr->message_id;

   
  //////////
  // We have subheaders; So identify correct ones.
  if (pqd->subhdrcount == 2) {
    int i;
    for(i=0; i < 2; ++i) {
      THubCtrlSubHeaderIdentificationHeader *idh = pqd->subhdrtable[i];
      switch(idh->subtype) {
      case KHubCtrlSubtypeRegisterAPService:
        raps = (void*)idh;
        break;
      case KHubCtrlSubtypeAuthentication:
        auth = (void*)idh;
        break;
      }
    }
  }

  //////////
  // Now, check that we got both hgeaders (Authentication is always required)
  if (raps && auth) {
    const int am = hubctrlcommonAuthenticateMessage((void*)reqhdr, reqsize, auth,
                                                    hc->APs_secret, hc->APs_secret_size,
                                                    hc->timestamp_difference_max);
    if (am == 0) {
      dPrint(ODTEST,"MESSAGE IS AUTHENTIC!");
      // Message is authentic! So; verification steps completed. Now,
      // carry on with the real assignment.

      // 1. Mark the entry as Authenticated APService link!
      OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkAPService);
      OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkAuthenticated);

      dPrint(ODTEST,"Got APService registration for %08x by type %08x",
             raps->requested_id, raps->service_type);

      // What to do? Reassign the ID.
      const uint32_t ap_part = (pqd->listentry->connectionID&0xFFFF0000);
      const uint32_t client_part = (raps->requested_id&0x0000FFFF);

      if (client_part >= hc->APs_reserved_pool_begin && 
          client_part <= hc->APs_reserved_pool_end) {
        const uint32_t target_id = ap_part | client_part;
        const int rar = connectionListReassignConnectionID(pqd->connectionlist,
                                                           pqd->listentry->connectionID,
                                                           target_id);
        if (rar == CONNECTIONLIST_OK) {
          rbase->result = KHubCtrlResultOK;
          assert(pqd->listentry->connectionID == target_id);
          assert(connectionListGetConnectionEntryByID(pqd->connectionlist, target_id) != NULL);
          
        } else {
          rbase->result = KHubCtrlResultIdReserved;
          
        }
      } else {
        rbase->result = KHubCtrlResultIdReserved;

      }
      
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) +
                                        sizeof(THubCtrlSubHeaderRegisterAPService) +
                                        sizeof(THubCtrlSubHeaderAuthentication) );
      rbase->subheader_count = 2;
      
      // Copy the RegisterAPService as-is.
      *rregs = *raps;
      
      // Update the authentication. (Server verifies
      rauth->subtype = KHubCtrlSubtypeAuthentication;
      rauth->subsize = sizeof(THubCtrlSubHeaderAuthentication);
      rauth->timestamp_s = time(NULL);
      memset(rauth->sha1, 0x00, sizeof(rauth->sha1));
      uint8_t sha1[20];
      unsigned int sha1_size = 20;
      apsecurityCalculateHMAC(sha1,
                              &sha1_size,
                              (uint8_t*)hc->APs_secret,
                              hc->APs_secret_size,
                              replybuffer,
                              rbase->protocol_id.total_size);
      assert(sha1_size == 20);
      memcpy(rauth->sha1, sha1, sizeof(sha1));

    
    } else if (am > 0) {
      // Positive value was less critical error; meaning expired timestamp
      dPrint(ODTEST,"Message had expired timestamp!");
      rbase->result = KHubCtrlResultAuthExpired;
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
      rbase->subheader_count = 0;
      
    } else {
      dPrint(ODTEST,"Message is corrupted or compromised!");
      // Negative value is hard error - something is fucked up.
      rbase->result = KHubCtrlResultAuthFailed;
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
      rbase->subheader_count = 0;
  
    }

  } else {
    rbase->result = KHubCtrlResultUnknownError;
    rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
    rbase->subheader_count = 0;
    
  }

  // Now, write the size out to enable sending reply.
  *pqd->replybuffer_datasize = rbase->protocol_id.total_size;
   
  return result;
}
//*****************************************************************************

EProtocolExit processRequestRegisterAPClient(THubCtrl *hc,
                                             const struct SProcessQueryData *pqd)
  
{
  EProtocolExit result = protocolExitError;
  const THubCtrlProtocolHeader *base = pqd->reqhdr;
  const unsigned int basesize = pqd->req_size;
  const THubCtrlSubHeaderAuthentication *auth = NULL;

  // Construct pointers to different elements. These are all
  // calculated compile-time.
  uint8_t *replybuffer = pqd->replybuffer;
  THubCtrlProtocolHeader *rbase = (void*)replybuffer;
  THubCtrlSubHeaderAuthentication *rauth = (void*)( replybuffer + 
                                                    sizeof(THubCtrlProtocolHeader) );
  
  //////////
  // Construct reply mainhdr (Must be done; not depending on result)
  rbase->protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  rbase->flags = KHubCtrlFlagReply;
  rbase->request = base->request;
  rbase->message_id = base->message_id;

   
  //////////
  // We have subheaders; So identify correct ones.
  if (pqd->subhdrcount == 1) {
    auth = (void*)pqd->subhdrtable[0];
  }

  //////////
  // Now, check that we got both hgeaders (Authentication is always required)
  if (auth) {
    const int am = hubctrlcommonAuthenticateMessage((void*)base, basesize, auth,
                                                    hc->client_secret, hc->client_secret_size,
                                                    hc->timestamp_difference_max);
    if (am == 0) {
      dPrint(ODTEST,"MESSAGE IS AUTHENTIC!");
      // Message is authentic! So; verification steps completed. Now,
      // carry on with the real assignment.

      // 1. Mark the entry as Authenticated AP2AP link!
      OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkAPClient);
      OWNFLAG_SET(pqd->listentry->flags, connectionEntryFlag_LinkAuthenticated);

      dPrint(ODTEST,"Got Succesfull APClient registration from NodeID 0x%08x",
             pqd->listentry->connectionID);
      
      // Do nothing - Just OK!
      rbase->result = KHubCtrlResultOK;

      // Construct reply
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) +
                                        sizeof(THubCtrlSubHeaderAuthentication) );
      rbase->subheader_count = 1;
      
      // Update the authentication. (Server verifies origin)
      rauth->subtype = KHubCtrlSubtypeAuthentication;
      rauth->subsize = sizeof(THubCtrlSubHeaderAuthentication);
      rauth->timestamp_s = time(NULL);
      memset(rauth->sha1, 0x00, sizeof(rauth->sha1));
      uint8_t sha1[20];
      unsigned int sha1_size = 20;
      apsecurityCalculateHMAC(sha1,
                              &sha1_size,
                              (uint8_t*)hc->client_secret,
                              hc->client_secret_size,
                              replybuffer,
                              rbase->protocol_id.total_size);
      assert(sha1_size == 20);
      memcpy(rauth->sha1, sha1, sizeof(sha1));

    
    } else if (am > 0) {
      // Positive value was less critical error; meaning expired timestamp
      dPrint(ODTEST,"Message had expired timestamp!");
      rbase->result = KHubCtrlResultAuthExpired;
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
      rbase->subheader_count = 0;
      
    } else {
      dPrint(ODTEST,"Message is corrupted or compromised!");
      // Negative value is hard error - something is fucked up.
      rbase->result = KHubCtrlResultAuthFailed;
      rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
      rbase->subheader_count = 0;
  
    }

  } else {
    rbase->result = KHubCtrlResultUnknownError;
    rbase->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
    rbase->subheader_count = 0;
    
  }

  // Now, write the size out to enable sending reply.
  *pqd->replybuffer_datasize = rbase->protocol_id.total_size;
   
  return result;
}
//*****************************************************************************

EProtocolExit processRequestUnknown(THubCtrl *hc,
                                    const struct SProcessQueryData *pqd)
{
  EProtocolExit result = protocolExitError;

  dPrint(ODTEST,"Received Unknown or invalid HubCtrl frame from NodeID: '0x0%08x'",
         pqd->listentry->connectionID);

  // Construct reply mainhdr.
  uint8_t *replybuffer = pqd->replybuffer;
  THubCtrlProtocolHeader *replyhdr = (THubCtrlProtocolHeader *)replybuffer;
  const THubCtrlProtocolHeader *reqhdr = pqd->reqhdr;
  replyhdr->protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  replyhdr->protocol_id.total_size = ( sizeof(THubCtrlProtocolHeader) );
  replyhdr->request = reqhdr->request;
  replyhdr->subheader_count = 0;
  replyhdr->flags = KHubCtrlFlagReply;
  replyhdr->result = KHubCtrlResultUnknownError;
  replyhdr->message_id = reqhdr->message_id;

  // Store size to caller.
  *pqd->replybuffer_datasize = replyhdr->protocol_id.total_size;
  
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


